import icommand.platform.nxt.Motor;
class NxtRover{
  /** Left Motor of NxTRover**/
  private Motor left;
  
  /** Left Motor of NxTRover **/
  private Motor right;
  
   /** Time/Count per degree it takes for the wheel to turn by a degree **/ 
  private double countPerDegree;
 
   /** Constructer: Initialize the Motors, and Count per Degree & sets default speed**/ 
  public NxtRover(double wheelDiameter, double driveLength, double ratio){
    
    //Initialize Motors
    left = Motor.C;
    right = Motor.A;
    
    // Calculate counts per degree
    double wheelCircumference = wheelDiameter * Math.PI;
    double fullRotation = (driveLength * Math.PI);
    countPerDegree = ((fullRotation/wheelCircumference)*ratio); 
 
    //set default speed
    left.setSpeed(50);
    right.setSpeed(50);
      
  }
  
  
  /** Move forward until overridden by another movement command * */
  public void moveForward(){
    left.forward();
    right.forward();  
  }
  
  /** Move backward until overridden by another movement command * */
  public void moveBackward(){
    left.backward();
    right.backward();   
  } 
  
   /** stops the motors left and right of the NxtRovers to come to complete halt */
  public void halt(){
    left.stop();
    right.stop();   
  }
  
  //stops the the rover smoothly (not suddent break)
  public void coast(){
    left.flt();
    right.flt();   
  }
   
   
   /** Change the current speed of the NxtRover to input parameter speed, which should be a integer number
     * between 0 and 100. * */
  public void changeRoverSpeed(int speed){
    if(speed > 0){
      left.setSpeed(speed);
      right.setSpeed(speed);
    }
    else{
      left.setSpeed(50);
      right.setSpeed(50);
    }
  }
  
  /** Turn the robot for the given number of milliseconds. This method is the basis of the
    * turnLeft() and turnRight() methods */
  
  protected void turn(int milliseconds) {
    
    try {
      if(milliseconds < 0) {
        
        left.forward();
        right.backward();
        milliseconds = -milliseconds;
      } 
      else {
   
        right.forward();
        left.backward();
      }
      Thread.sleep(milliseconds);
      halt();
    } 
    catch (InterruptedException e) {}
  }
  
  /** Turn towards the left (counter-clockwise) by degrees, or clockwise if negative. */
   public void turnLeft(double degrees) {
        int count = (int)(countPerDegree * degrees);
        turn(count);
    }
   
   /** Turn towards the right (clockwise) by degrees, or counter-clockwise if negative. */
   public void turnRight(double degrees) {
     int count = (int)(countPerDegree * degrees);
     turn(-count);
   }
  
   /** This method is basis for the methods moveForward(int milliseconds) and moveBackward(int milliseconds) * */
   protected void move(int milliseconds)
    throws InterruptedException {
    if( milliseconds >= 0 ) {
 
        left.forward();
        right.forward();
    } else {
    
        left.backward();
        right.backward();
        milliseconds = -milliseconds;
    }
    Thread.sleep(milliseconds);
        halt();
    }
    
    /** Move forward for approximately milliseconds * */
    public void moveForward(int milliseconds) {
        try {
            move(milliseconds);
            
        } catch (InterruptedException e) {}
    }

      /** Move backward for approximately milliseconds*/
    public void moveBackward(int milliseconds) {
        try {
            move(-milliseconds);
            
        } catch (InterruptedException e) {}
    }
}